//
// Copyright (c) Microsoft Corporation.  All rights reserved.
//
//
// Use of this sample source code is subject to the terms of the Microsoft
// license agreement under which you licensed this sample source code. If
// you did not accept the terms of the license agreement, you are not
// authorized to use this sample source code. For the terms of the license,
// please see the license agreement between you and Microsoft or, if applicable,
// see the LICENSE.RTF on your install media or the root of your tools installation.
// THE SAMPLE SOURCE CODE IS PROVIDED "AS IS", WITH NO WARRANTIES OR INDEMNITIES.
//
//
// (C) Copyright 2006 Marvell International Ltd.
// All Rights Reserved
//
/******************************************************************************
**
** INTEL CONFIDENTIAL
** Copyright 2003-2005 Intel Corporation All Rights Reserved.
**
** The source code contained or described herein and all documents
** related to the source code (Material) are owned by Intel Corporation
** or its suppliers or licensors.  Title to the Material remains with
** Intel Corporation or its suppliers and licensors. The Material contains
** trade secrets and proprietary and confidential information of Intel
** or its suppliers and licensors. The Material is protected by worldwide
** copyright and trade secret laws and treaty provisions. No part of the
** Material may be used, copied, reproduced, modified, published, uploaded,
** posted, transmitted, distributed, or disclosed in any way without Intel's
** prior express written permission.
**
** No license under any patent, copyright, trade secret or other intellectual
** property right is granted to or conferred upon you by disclosure or
** delivery of the Materials, either expressly, by implication, inducement,
** estoppel or otherwise. Any license under such intellectual property rights
** must be express and approved by Intel in writing.
**
**
******************************************************************************/

#include <windows.h>
#include <nkintr.h>
#include <monahans.h>
#include <Littleton.h>
#include <oal_memory.h>
#include <pcireg.h>
#include <fmd.h>
#include <bsp.h>
#include "loader.h"
#include <bootpart.h>
#include "xllp_pm.h"
#include "xllp_pm_dvfm.h"
#include "monahans_base_regs.h"
#include "xllp_defs.h"
#include "xllp_udc.h"
#include "xllp_lcd_plat.h"
#include "ipm_xllp_pm_dvfm.h"

#include <flash_cfg.h>
#include <blcommon.h>

//Included for the SD/MMC boot option
#include "SDMMC.h"
#include "FAT.h"

#include "BLMenu.h"

// #include "xllp_defs.h"
#include "xllp_lcd.h"
#include "xllp_ost.h"
#include "xllp_lcd_plat.h"
//#include "pxa2d_gcu.h"
#include "loader.h"

#include "xllp_pm_Micco.h"
#include "xllp_i2c.h"
#include "xllp_mfp_proc.h"
#include "xllp_mfp_plat.h"

//------------------------------------------------------------------------------
// External function prorotypes.
//
extern BOOL g_JumpDirectlyIntoRAM;

extern UINT16 hex_atoi(const char * hexstr);

void OEMPowerOff();
void DumpMFPDomainAuto();
extern BSP_ARGS *g_pBSPArgs;
extern FlashInfo g_FlashInfo;
extern EBOOT_CFG    *g_pEbootCFG ;

#define	NUM_READ_WRITES			100000
#define	NUM_READ_WRITES_SHORT	10
#define	BUFF_SIZE				0x01000000	// 16MB

#define DMC_REG_BASE	0x48100000 
#define MDCNFG_REG		0x000 
#define MDREFR_REG		0x004 
#define MDMRS_REG		0x040 
#define DDR_SCAL_REG	0x050	
#define DDR_HCAL_REG	0x060	
#define DDR_WCAL_REG	0x068	
#define DMCIER_REG		0x070	
#define DMCISR_REG		0x078	
#define	EMPI_REG		0x090
#define RCOMP_REG		0x100
#define	PAD_MA_REG   	0x110
#define	PAD_MDMSB_REG	0x114
#define	PAD_MDLSB_REG	0x118
#define	PAD_SDRAM_REG	0x11C
#define	PAD_SDCLK_REG	0x120
#define	PAD_SDCS_REG	0x124
#define	PAD_SMEM_REG	0x128
#define PAD_SCLK_REG	0x12C

//------------------------------------------------------------------------------
//
//  Function:  DumpTOC
//
//  Dumps a ROMHDR.
//
void DumpTOC(const ROMHDR* pTOC)
{
    EdbgOutputDebugString("pTOC:\r\n");
    EdbgOutputDebugString("\tDLL First      : 0x%X\r\n", pTOC->dllfirst);
    EdbgOutputDebugString("\tDLL last       : 0x%X\r\n", pTOC->dlllast);
    EdbgOutputDebugString("\tPhys First     : 0x%X\r\n", pTOC->physfirst);
    EdbgOutputDebugString("\tPhys Last      : 0x%X\r\n", pTOC->physlast);
    EdbgOutputDebugString("\tnummods        : 0x%X (%d) (# of TOCs)\r\n", pTOC->nummods, pTOC->nummods);
    EdbgOutputDebugString("\tulRamStart     : 0x%X\r\n", pTOC->ulRAMStart);
    EdbgOutputDebugString("\tulRAMEnd       : 0x%X\r\n", pTOC->ulRAMEnd);
    EdbgOutputDebugString("\tulRAMFree      : 0x%X\r\n", pTOC->ulRAMFree);
    EdbgOutputDebugString("\tulCopyEntries  : 0x%X (%d) (# of copy entries)\r\n", pTOC->ulCopyEntries, pTOC->ulCopyEntries);
    EdbgOutputDebugString("\tulCopyOffset   : 0x%X\r\n", pTOC->ulCopyOffset);
    EdbgOutputDebugString("\tulProfileLen   : 0x%X\r\n", pTOC->ulProfileLen);
    EdbgOutputDebugString("\tulProfileOffset: 0x%X\r\n", pTOC->ulProfileOffset);
    EdbgOutputDebugString("\tnumfiles       : 0x%X (%d)\r\n", pTOC->numfiles, pTOC->numfiles);
    EdbgOutputDebugString("\tulKernelFlags  : 0x%X\r\n", pTOC->ulKernelFlags);
    EdbgOutputDebugString("\tulFSRamPercent : 0x%X\r\n", pTOC->ulFSRamPercent);
    EdbgOutputDebugString("\tulDrivglobStart: 0x%X\r\n", pTOC->ulDrivglobStart);
    EdbgOutputDebugString("\tulDrivglobLen  : 0x%X\r\n", pTOC->ulDrivglobLen);
    EdbgOutputDebugString("\tusCPUType      : 0x%H\r\n", pTOC->usCPUType);
    EdbgOutputDebugString("\tusMiscFlags    : 0x%H\r\n", pTOC->usMiscFlags);
    EdbgOutputDebugString("\tpExtensions    : 0x%X\r\n", pTOC->pExtensions);
    EdbgOutputDebugString("\tulTrackingStart: 0x%X\r\n", pTOC->ulTrackingStart);
    EdbgOutputDebugString("\tulTrackingLen  : 0x%X\r\n", pTOC->ulTrackingLen);
}


void DumpRegistersDDR(void)
{
    volatile UINT8* pDFCReg = (volatile UINT8* )OALPAtoVA(DMC_REG_BASE, FALSE);

    EdbgOutputDebugString ("DDR Registers      Read         Expected\r\n\r\n");
    EdbgOutputDebugString ("MDCNFG		: 0x%x 0x8000072D\r\n", *(UINT32*)(pDFCReg + MDCNFG_REG));		 
    EdbgOutputDebugString ("MDREFR		: 0x%x 0x00000019\r\n", *(UINT32*)(pDFCReg + MDREFR_REG)); 
    EdbgOutputDebugString ("MDMRS       : 0x%x 0x40000033\r\n", *(UINT32*)(pDFCReg + MDMRS_REG)); 
    EdbgOutputDebugString ("DDR_SCAL	: 0x%x 0x00000F0F\r\n", *(UINT32*)(pDFCReg + DDR_SCAL_REG));	
    EdbgOutputDebugString ("DDR_HCAL	: 0x%x 0x90000000\r\n", *(UINT32*)(pDFCReg + DDR_HCAL_REG));	
    EdbgOutputDebugString ("DDR_WCAL	: 0x%x 0x00000100\r\n", *(UINT32*)(pDFCReg + DDR_WCAL_REG));	
    EdbgOutputDebugString ("DMCIER		: 0x%x 0x00000000\r\n", *(UINT32*)(pDFCReg + DMCIER_REG));	
    EdbgOutputDebugString ("DMCISR		: 0x%x 0x00000000\r\n", *(UINT32*)(pDFCReg + DMCISR_REG));	
    EdbgOutputDebugString ("EMPI		: 0x%x 0xC5000000\r\n", *(UINT32*)(pDFCReg + EMPI_REG));
    EdbgOutputDebugString ("PAD_MA  	: 0x%x 0x22300404\r\n", *(UINT32*)(pDFCReg + PAD_MA_REG));
    EdbgOutputDebugString ("PAD_MDMSB	: 0x%x 0x22300808\r\n", *(UINT32*)(pDFCReg + PAD_MDMSB_REG));
    EdbgOutputDebugString ("PAD_MDLSB	: 0x%x 0x22300808\r\n", *(UINT32*)(pDFCReg + PAD_MDLSB_REG));
    EdbgOutputDebugString ("PAD_SDRAM	: 0x%x 0x22300404\r\n", *(UINT32*)(pDFCReg + PAD_SDRAM_REG));
    EdbgOutputDebugString ("PAD_SDCLK	: 0x%x 0x22300404\r\n", *(UINT32*)(pDFCReg + PAD_SDCLK_REG));
    EdbgOutputDebugString ("PAD_SDCS	: 0x%x 0x22300404\r\n", *(UINT32*)(pDFCReg + PAD_SDCS_REG));
    EdbgOutputDebugString ("PAD_SMEM	: 0x%x 0x22300404\r\n", *(UINT32*)(pDFCReg + PAD_SMEM_REG));
    EdbgOutputDebugString ("PAD_SCLK	: 0x%x 0x22300404\r\n", *(UINT32*)(pDFCReg + PAD_SCLK_REG));
    EdbgOutputDebugString ("RCOMP		: 0x%x 0x42000000\r\n", *(UINT32*)(pDFCReg + RCOMP_REG));
}

void WriteReadDDR(UINT32 numCycles);

/// Load image from DDR.
///
/// @param  enableJump          [IN]whethere jump to image
///
/// @remarks  if it's for WinCE, load the OS. If it's for Windowms Mobile, load IPL.
void LoadImageDDR(BOOL enableJump)
{
#ifdef OSV_WINCENET
    //BLOCK_ID startBlock = FLASH_BLOCK_OS_START;
    BLOCK_ID startBlock;
    UINT8 *pDataBuffer = (UINT8 *)OS_RAM_IMAGE_START;
    UINT32 PhysAddress;
    //UINT32 numBlocks = OS_RAM_IMAGE_SIZE_NUM_BLOCKS;
    UINT32 numBlocks;

    if (g_FlashInfo.wDataBytesPerSector == LB_BYTES_PER_PAGE)
    {
        startBlock = LB_FLASH_BLOCK_OS_START;
        numBlocks = LB_OS_RAM_IMAGE_SIZE_NUM_BLOCKS;
    }

    else
    {
        startBlock = FLASH_BLOCK_OS_START;
        numBlocks = OS_RAM_IMAGE_SIZE_NUM_BLOCKS;
    }
#else
    //BLOCK_ID startBlock = OS_FLASH_IMAGE_START_BLOCK;
    // Now always launch IPL first
    //BLOCK_ID startBlock = g_pEbootCFG->dwPhyStartBlock;   
    //UINT8 *pDataBuffer = (UINT8 *)OS_RAM_IMAGE_START;
    BLOCK_ID startBlock;

    UINT8 *pDataBuffer = (UINT8 *)IMAGE_IPL_RAM_UA_START;

    UINT32 PhysAddress;

    //UINT32 numBlocks = OS_RAM_IMAGE_SIZE_NUM_BLOCKS;
    //
    //UINT32 numBlocks = g_pEbootCFG->dwPhyNumBlocks;
    UINT32 numBlocks;

    if (g_FlashInfo.wDataBytesPerSector == LB_BYTES_PER_PAGE)
    {
        startBlock = LB_FLASH_BLOCK_IPL_START;
        numBlocks = LB_FLASH_BLOCK_IPL_LENGTH;
    }

    else
    {
        startBlock = FLASH_BLOCK_IPL_START;
        numBlocks = FLASH_BLOCK_IPL_LENGTH;
    }
#endif

    // BLCOMMON calls this...
    if (!g_JumpDirectlyIntoRAM)
    {
        EdbgOutputDebugString ("Loading Image from Flash...\r\n");
        EdbgOutputDebugString ("Start block: %d\r\n", startBlock);
        EdbgOutputDebugString ("Number of blocks: %d\r\n", numBlocks);
        EdbgOutputDebugString ("Image start address in RAM: 0x%x\r\n", pDataBuffer);

        if (!LoadImageFromFlash((BLOCK_ID)startBlock, pDataBuffer, numBlocks))
        {
            EdbgOutputDebugString ("Loading Image from Flash Failed!\r\n");
            return;
        }

        EdbgOutputDebugString ("Loading Image from Flash Done!\r\n");
    }
    else
    {
        EdbgOutputDebugString ("Using existing image in RAM\r\n");
    }

    // Quick hack to find pTOC in image
    do
    {
        // EBoot and the kernel's view of RAM differs. This is because the CE OEMAddressTable
        // maps RAM to 0x80000000, while in Eboot, it's that 0x84000000 (as Eboot doesn't
        // work in RAM, but the SRAM of Monohans).
        // Launch IPL.bin instead of NK.bin
        //DWORD* ram_ptr = (DWORD*) 0xA0100000;
#ifdef OSV_WINCENET
        DWORD* ram_ptr = (DWORD*)(OS_RAM_IMAGE_START + 0x20000000);
#else
        DWORD* ram_ptr = (DWORD*) IMAGE_IPL_RAM_UA_START;
#endif
        DWORD* ptoc_area = ram_ptr + 16; // 64 bytes in is the NK signature area, I think
        DWORD  ptoc_addr;
        ROMHDR* nk_pTOC;

        EdbgOutputDebugString("Print out pTOC location from beginning (offset 0x40)\r\n");
        EdbgOutputDebugString("Signature: 0x%X\r\n", *(ptoc_area + 0));
        EdbgOutputDebugString("Virt Addr: 0x%X\r\n", *(ptoc_area + 1));
        EdbgOutputDebugString("Offset   : 0x%X\r\n", *(ptoc_area + 2));

        // Attempt to decode pTOC area
        ptoc_addr = *(ptoc_area + 1);
        // Adjust for RAM offset
        //ptoc_addr += 0x4000000;
        // Our pointer.
        nk_pTOC = (ROMHDR*) ptoc_addr;

        // dump pTOC
        DumpTOC(nk_pTOC);
    } while (0);

    if (enableJump == TRUE) {
        // Translate the image start address (virtual address) to a physical address.
        //
        PhysAddress = (UINT32) OALVAtoPA((VOID *)pDataBuffer);
        EdbgOutputDebugString("Download successful!  Jumping to image at 0x%x (physical 0x%x)...\r\n", pDataBuffer, PhysAddress);

    // Jump to the image we just downloaded.
    //
    EdbgOutputDebugString("\r\n\r\n\r\n");
    Launch(PhysAddress);

    // Should never get here...
    //
    SpinForever();

    }
}


void JumpToImageDDR(void)
{
    UINT8 *pDataBuffer = (UINT8 *)OS_RAM_IMAGE_START;
    UINT32 PhysAddress;

    if (g_pEbootCFG->dwLaunchAddr)
    {
        pDataBuffer = (UINT8 *) g_pEbootCFG->dwLaunchAddr;
    }

    // Quick hack to find pTOC in image
    do
    {
        // EBoot and the kernel's view of RAM differs. This is because the CE OEMAddressTable
        // maps RAM to 0x80000000, while in Eboot, it's that 0x84000000 (as Eboot doesn't
        // work in RAM, but the SRAM of Monohans).
        DWORD* ram_ptr = (DWORD*) IMAGE_IPL_RAM_UA_START;
        DWORD* ptoc_area = ram_ptr + 16; // 64 bytes in is the NK signature area, I think
        DWORD  ptoc_addr;
        ROMHDR* nk_pTOC;

        EdbgOutputDebugString("Print out pTOC location from beginning (offset 0x40)\r\n");
        EdbgOutputDebugString("Signature: 0x%X\r\n", *(ptoc_area + 0));
        EdbgOutputDebugString("Virt Addr: 0x%X\r\n", *(ptoc_area + 1));
        EdbgOutputDebugString("Offset   : 0x%X\r\n", *(ptoc_area + 2));

        // Attempt to decode pTOC area
        ptoc_addr = *(ptoc_area + 1);
        // Adjust for RAM offset
        //ptoc_addr += 0x4000000;
        // Our pointer.
        nk_pTOC = (ROMHDR*) ptoc_addr;

        // dump pTOC
        DumpTOC(nk_pTOC);
    } while (0);

    // Translate the image start address (virtual address) to a physical address.
    //
    PhysAddress = (UINT32) OALVAtoPA((VOID *)pDataBuffer);
    EdbgOutputDebugString("Download successful!  Jumping to image at 0x%x (physical 0x%x)...\r\n", pDataBuffer, PhysAddress);


    // Jump to the image we just downloaded.
    //
    EdbgOutputDebugString("\r\n\r\n\r\n");
    Launch(PhysAddress);

    // Should never get here...
    //
    SpinForever();
}

/// Read specific address of DDR
/// @remarks  only for test purpose
void ReadSimpleDDR(void)
{
    volatile DWORD *pDataBuffer = (volatile DWORD *) OALPAtoVA(OS_RAM_IMAGE_START, FALSE);
    DWORD count;
    DWORD index;

    count = (OS_RAM_END - OS_RAM_IMAGE_START) / sizeof(DWORD);
    index = 0;

    while (index++ < count)
    {
        if ((DWORD) * pDataBuffer != (DWORD) pDataBuffer)
            EdbgOutputDebugString("\r\nAddress 0x%X not the same (Wanted 0x%X, Got 0x%X)\r\n", pDataBuffer, pDataBuffer, *pDataBuffer);
        pDataBuffer++;
        if (!((DWORD) pDataBuffer & 0xFFFFF))
        {
            OEMWriteDebugByte('\r');
            EdbgOutputDebugString("0x%X", pDataBuffer);
        }
    }

    EdbgOutputDebugString("Read DDR Done!\r\n");
}

/// Write specific address of DDR
/// @remarks  only for test purpose
void WriteSimpleDDR(void)
{
    volatile DWORD *pDataBuffer = (volatile DWORD *) OALPAtoVA(OS_RAM_IMAGE_START, TRUE);
    DWORD count;
    DWORD index;

    count = (OS_RAM_END - OS_RAM_IMAGE_START) / sizeof(DWORD);
    index = 0;

    while (index++ < count)
    {
        *pDataBuffer = (DWORD) pDataBuffer;
        pDataBuffer++;
        if (!((DWORD) pDataBuffer & 0xFFFFF))
        {
            OEMWriteDebugByte('\r');
            EdbgOutputDebugString("0x%X", pDataBuffer);
        }
    }

    EdbgOutputDebugString("Write DDR Done!\r\n");
}

BOOL WriteReadSilent()
{
    volatile DWORD *pDataBuffer = (volatile DWORD *) OALPAtoVA(OS_RAM_IMAGE_START, TRUE);
    DWORD count;
    DWORD index;
    BOOL retVal;

    //count = (OS_RAM_END - OS_RAM_IMAGE_START) / sizeof(DWORD);
    count = 0x100000 / sizeof(DWORD);
    index = 0;

    /*// write
    while (index++ < count)	
    {
        *pDataBuffer = (DWORD) pDataBuffer;	
        pDataBuffer++;
    }
    index = 0; */
    retVal = TRUE;
    pDataBuffer = (volatile DWORD *) OALPAtoVA(OS_RAM_IMAGE_START, TRUE);
    // read
    while (index++ < count)
    {
        if ((DWORD) * pDataBuffer != (DWORD) pDataBuffer)
        {
            EdbgOutputDebugString("\r\nAddress 0x%X not the same (Wanted 0x%X, Got 0x%X)\r\n", pDataBuffer, pDataBuffer, *pDataBuffer);
            retVal = FALSE;
            break;
        }
        if (!((DWORD) pDataBuffer & 0xFFFF)) 
        {
            OEMWriteDebugByte('\r');
            EdbgOutputDebugString("0x%X", pDataBuffer);
        }
        pDataBuffer++;

    }

    return retVal;

}

/// Read/Write specific address of DDR
/// @remarks  only for test purpose
void WriteReadDDR(UINT32 numCycles)
{
    UINT32 i, j, k=0, errorCount=0, errorCountTotal=0;
    volatile UINT8 *pDataBuffer = (UINT8 *)OS_RAM_IMAGE_START;
    volatile UINT8 tempW = 0;
    volatile UINT8 tempR = 0;

    for(j = 0; j < numCycles; j++)
    {
        k ^= 1;
        OEMWriteDebugLED(0, k);

        pDataBuffer = (UINT8 *)OS_RAM_IMAGE_START;
        for(i = 0; i < BUFF_SIZE; i++)
        {
            *pDataBuffer = tempW;
            tempR = *pDataBuffer;
//			EdbgOutputDebugString("test cycle: %d  buffer: 0x%x  Wrote: 0x%x  Read: 0x%x\r\n",
//		 								j, pDataBuffer, tempW, tempR);

            if (tempW != tempR)
            {
                EdbgOutputDebugString("test cycle: %d  buffer: 0x%x  Wrote: 0x%x  Read: 0x%x\r\n",
                                        j+1, pDataBuffer, tempW, tempR);
                errorCount++;
            }
            pDataBuffer++;
            tempW = (i & 0xF)<<4 | (i & 0xF);
        }
        tempW = (j & 0xF)<<4 | (j & 0xF);

        errorCountTotal += errorCount;
        EdbgOutputDebugString("test cycle: %d, error count: %d\r\n",	j+1, errorCount);
    }
    EdbgOutputDebugString("Write/Read DDR Done, total error count:%d\r\n", errorCountTotal);
}

void TestLED(void)
{
    P_XLLP_OST_T pOSTReg = (P_XLLP_OST_T) OALPAtoVA(MONAHANS_BASE_REG_PA_OST, FALSE);

    EdbgOutputDebugString("Set LED\r\n");
    OEMWriteDebugLED(0, 1);
    OEMWriteDebugLED(1, 1);
    XllpOstDelayMilliSeconds(pOSTReg, 1000);

    EdbgOutputDebugString("Clear LED\r\n");
    OEMWriteDebugLED(0, 0);
    OEMWriteDebugLED(1, 0);
}

/// Get user input DWORD type.
/// @return value of user input 
DWORD GetDWORD(void)
{
    char input[9];
    WORD cwNumChars = 0;
    UINT16 InChar = 0;
    DWORD value = 0;

    memset(input, 0, sizeof(input));

    while (!((InChar == 0x0d) || (InChar == 0x0a)))
    {
        InChar = OEMReadDebugByte();
        if (InChar != OEM_DEBUG_COM_ERROR && InChar != OEM_DEBUG_READ_NODATA)
        {
            // If it's a number or A-F, add it to the string
            if (((InChar >= '0') && (InChar <= '9')) || 
                ((InChar >= 'a') && (InChar <= 'f')) ||
                ((InChar >= 'A') && (InChar <= 'F')))
            {
                if (cwNumChars < 8)
                {
                    input[cwNumChars++] = (char) (InChar & 0x00FF);
                    OEMWriteDebugByte((BYTE) (InChar & 0x00FF));
                }
            }
            // If backspace, backup
            else if (InChar == 8)
            {
                if (cwNumChars > 0)
                {
                    cwNumChars--;
                    input[cwNumChars] = 0;
                    OEMWriteDebugByte((BYTE)(InChar & 0x00FF));
                    OEMWriteDebugByte(' ');
                    OEMWriteDebugByte((BYTE)(InChar & 0x00FF));
                }
            }
        }
    }

    // If empty string, return 0
    if (cwNumChars)
    {
        // hex_atoi only works with words, so we have to split it.
        char input_split[10];
        WORD high, low;
        memset(input_split, 0, sizeof(input_split));
        // Copy first half of DWORD out.
        memcpy(&(input_split[0]), &(input[0]), 4);
        // Copy second half
        memcpy(&(input_split[5]), &(input[4]), 4);

        // Convert
        high = hex_atoi(&(input_split[0]));
        low  = hex_atoi(&(input_split[5]));
        value = ((DWORD) high) << 16 | low;
    }

    return value;
}



#define MAX_SECTOR_NUM 65535
int GetSectorAddr(void)
{
    char szCount[16];
    WORD cwNumChars = 0;
    UINT16 InChar = 0;
    int value;

    EdbgOutputDebugString("\r\nEnter sector number (0-%d)", MAX_SECTOR_NUM);

    while (!((InChar == 0x0d) || (InChar == 0x0a)))
    {
        InChar = OEMReadDebugByte();
        if (InChar != OEM_DEBUG_COM_ERROR && InChar != OEM_DEBUG_READ_NODATA)
        {
            // If it's a number or a period, add it to the string
            if ((InChar >= '0' && InChar <= '9'))
            {
                if (cwNumChars < 16)
                {
                    szCount[cwNumChars++] = (char)InChar;
                    OEMWriteDebugByte((BYTE)InChar);
                }
            }
            // If it's a backspace, back up
            else if (InChar == 8)
            {
                if (cwNumChars > 0)
                {
                    cwNumChars--;
                    OEMWriteDebugByte((BYTE)InChar);
                }
            }
        }
    }

    // If it's a carriage return with an empty string, don't change anything.
    if (cwNumChars)
    {
        szCount[cwNumChars] = '\0';
        value = atoi(szCount);
        if (value > MAX_SECTOR_NUM)
        {
            //EdbgOutputDebugString("Exceed max use %d instead\r\n", MAX_SECTOR_NUM);
            value = MAX_SECTOR_NUM;
        }
        else if (value < 0)
        {
            //EdbgOutputDebugString("Below min use %d instead\r\n", 0);
            value = 0;
        }
    }

    return value;
}

int GetBlockAddr(int start, int end)
{
    char szCount[16];
    WORD cwNumChars = 0;
    UINT16 InChar = 0;
    int value;

    EdbgOutputDebugString ( "\r\nEnter BlockID (%d-%d)", start, end);

    while (!((InChar == 0x0d) || (InChar == 0x0a)))
    {
        InChar = OEMReadDebugByte();
        if (InChar != OEM_DEBUG_COM_ERROR && InChar != OEM_DEBUG_READ_NODATA)
        {
            // If it's a number or a period, add it to the string
            if ((InChar >= '0' && InChar <= '9'))
            {
                if (cwNumChars < 16)
                {
                    szCount[cwNumChars++] = (char)InChar;
                    OEMWriteDebugByte((BYTE)InChar);
                }
            }
            // If it's a backspace, back up
            else if (InChar == 8)
            {
                if (cwNumChars > 0)
                {
                    cwNumChars--;
                    OEMWriteDebugByte((BYTE)InChar);
                }
            }
        }
    }

    // If it's a carriage return with an empty string, don't change anything.
    if (cwNumChars)
    {
        szCount[cwNumChars] = '\0';
        value = atoi(szCount);
        if (value > end)
        {
            value = end;
        }
        else if (value < start)
        {
            value = start;
        }
    }

    return value;
}

void dumpSectorInfo (SectorInfo *pSectorInfo)
{
    RETAILMSG(1, (TEXT("Sector Info\r\n")));
    RETAILMSG(1, (TEXT("===========\r\n")));
    RETAILMSG(1, (TEXT("bOEMReserved: 0x%x\r\n"), pSectorInfo->bOEMReserved));
    RETAILMSG(1, (TEXT("dwReserved1: 0x%x\r\n"), pSectorInfo->dwReserved1));
    RETAILMSG(1, (TEXT("wReserved2: 0x%x\r\n"), pSectorInfo->wReserved2));
    RETAILMSG(1, (TEXT("bBadBlock: 0x%x\r\n"), pSectorInfo->bBadBlock));
    
}
#define NUM_PARTS	4
#define PARTTABLE_OFFSET        (510 - (sizeof(PARTENTRY) * NUM_PARTS))
void DumpPartitionTable(UINT8 * pDataBuffer)
{
    PPARTENTRY pPartEntry = (PPARTENTRY) (pDataBuffer + PARTTABLE_OFFSET);
    DWORD iEntry = 0;

    for (iEntry = 0; iEntry < NUM_PARTS; iEntry++, pPartEntry++) {
        // Dump Partition Table
        RETAILMSG(1, (TEXT("---------------------\r\n")));
        RETAILMSG(1, (TEXT("Partition Information\r\n")));
        RETAILMSG(1, (TEXT("BootInd:	0x%x\r\n"), pPartEntry->Part_BootInd));
        RETAILMSG(1, (TEXT("FirstHead:	0x%x\r\n"), pPartEntry->Part_FirstHead));
        RETAILMSG(1, (TEXT("FirstSector:0x%x\r\n"), pPartEntry->Part_FirstSector));
        RETAILMSG(1, (TEXT("FirstTrack :0x%x\r\n"), pPartEntry->Part_FirstTrack));
        RETAILMSG(1, (TEXT("FileSystem:	0x%x\r\n"), pPartEntry->Part_FileSystem));
        RETAILMSG(1, (TEXT("LastHead:	0x%x\r\n"), pPartEntry->Part_LastHead));
        RETAILMSG(1, (TEXT("LastSector:	0x%x\r\n"), pPartEntry->Part_LastSector));
        RETAILMSG(1, (TEXT("LastTrack:	0x%x\r\n"), pPartEntry->Part_LastTrack));
        RETAILMSG(1, (TEXT("StartSector:0x%x\r\n"), pPartEntry->Part_StartSector));
        RETAILMSG(1, (TEXT("TotalSectors:	0x%x\r\n"), pPartEntry->Part_TotalSectors));
        RETAILMSG(1, (TEXT("----------------------------\r\n")));
    }
}

// config for external wake[0] and OST
//
//#define WAKE_MASK 0x40000001
#define WAKE_MASK 0x00000001

//void EnterD2(void *StartIMAddress, void *DMCBaseAddress, void *IMStackAddress);
typedef struct __IPMD1D2StructTest
{
    UINT32 ReloStartAdx;
    UINT32 DMCBaseAdx;
    UINT32 ReloSPAdx;
    UINT32 CP14Val;
} IPMD1D2StructTest;

void EnterLPM(IPMD1D2StructTest *IpmD1D2Struct);


// *********************************** Mini-LCD Testing ******************************

#define MINI_LCD_OFFSET 0x10000
#define MINI_LCD_FRAME_PA (MONAHANS_BASE_REG_PA_IMSTORAGE + MINI_LCD_OFFSET)
#define IM_SIZE 0x40000 	// in Monahans L, sram size is 256k

XLLP_MINI_LCD_T                 xllp_mini_lcd;
volatile UINT32*                LCD_FDADR0;
volatile XLLP_LCD_REGISTERS_T*  LCDRegs;

UINT32 lccr[7];
UINT32 fdadr0;

void lcd_reg_save()
{
    LCDRegs->lccr0 &= ~XLLP_LCCR0_ENB;

    lccr[0] = LCDRegs->lccr0;
    lccr[1] = LCDRegs->lccr1;
    lccr[2] = LCDRegs->lccr2;
    lccr[3] = LCDRegs->lccr3;
    lccr[4] = LCDRegs->lccr4;
    lccr[5] = LCDRegs->lccr5;
    lccr[6] = LCDRegs->lccr6;
    fdadr0 = LCDRegs->fdadr0;
}

void lcd_reg_restore()
{
    LCDRegs->lccr0 = lccr[0];
    LCDRegs->lccr1 = lccr[1];
    LCDRegs->lccr2 = lccr[2];
    LCDRegs->lccr3 = lccr[3];
    LCDRegs->lccr4 = lccr[4];
    LCDRegs->lccr5 = lccr[5];
    LCDRegs->lccr6 = lccr[6];
    LCDRegs->fdadr0 = fdadr0;

    LCDRegs->lccr0 |= XLLP_LCCR0_ENB;
}

UINT32 get_base_frame()
{
    LCD_FRAME_DESCRIPTOR *descriptor;
    UINT32 frame_phy_addr;

    descriptor = (LCD_FRAME_DESCRIPTOR*)OALPAtoVA(*LCD_FDADR0, TRUE);
    frame_phy_addr = descriptor->FSADR;
    return (UINT32)OALPAtoVA(frame_phy_addr, TRUE);
} 

void mini_lcd_on()
{
    unsigned int buf_size;
    UINT32 frame = get_base_frame();

    XllpMiniLCDEncode(&xllp_mini_lcd, frame, &buf_size);
    XllpMiniLCDEnable(&xllp_mini_lcd);
}

void mini_lcd_off()
{
    XllpMiniLCDDisable(&xllp_mini_lcd);
}

// Main LCDC init code
//
// 
#define BMAN_FB_PHY_ADX 0x80200000
#define DMA_CHANNEL_NUM 8
#define VIDEO_MEM_SIZE 0x300000
#define DMA_CHANNEL_UNUSED 7
#define IM_LCD_OFFSET 0x20000
#define IM_LCD_BASE_PHY	(MONAHANS_BASE_REG_PA_IMSTORAGE + IM_LCD_OFFSET)
#define IM_LCD_SIZE 0x96000
#define SMART_CMD_BUF_SIZE	2048

LCD_FRAME_DESCRIPTOR *dma_descriptors = NULL;
LCD_FRAME_DESCRIPTOR *dma_descriptors_phy = NULL;
unsigned short *cmd_buf = NULL;
unsigned short *cmd_buf_phy = NULL;
unsigned int *video_mem = NULL;
unsigned int *video_mem_phy = NULL;
unsigned int *sram_video_mem = NULL;

void DrawBuffer(void)
{
    UINT32 *fb = (UINT32 *) BMAN_FB_PHY_ADX;    // realize fhy==vir
    int x, line;

    // R
    for(line=0; line<105; line++)
        for(x=0; x<120; x++)
            *fb++ = 0xF800F800;
    // G
    for(line=0; line<105; line++)
        for(x=0; x<120; x++)
            *fb++ = 0x07E007E0;
    // B
    for(line=0; line<110; line++)
        for(x=0; x<120; x++)
            *fb++ = 0x001F001F;
}

static int alloc_map_mem()
{
    char *buf_phy;
    char *buf;
    unsigned int descriptor_size = sizeof(LCD_FRAME_DESCRIPTOR) * DMA_CHANNEL_NUM;
    unsigned int total = SMART_CMD_BUF_SIZE + VIDEO_MEM_SIZE + descriptor_size;

    buf = (char *) BMAN_FB_PHY_ADX;
    buf_phy = (char *) BMAN_FB_PHY_ADX;

    (char *) video_mem = buf;
    (char *) video_mem_phy = buf_phy;

    (char *) cmd_buf = buf + VIDEO_MEM_SIZE;
    (char *) cmd_buf_phy = buf_phy + VIDEO_MEM_SIZE;

    (char *) dma_descriptors = (char *) cmd_buf + SMART_CMD_BUF_SIZE;
    (char *) dma_descriptors_phy = (char *) cmd_buf_phy + SMART_CMD_BUF_SIZE;

    return 0;
}

static void LocalTFTInitController(P_XLLP_LCD_T pXllpLCD)
{
    tft_timing_t *timing = &pXllpLCD->panel->tft.timing;
    tft_polarity_t *polarity = &pXllpLCD->panel->tft.polarity;
    XLLP_LCD_REGISTERS_T *pLCDRegs;

    LCDInitControllerCommon(pXllpLCD);

    pLCDRegs = (XLLP_LCD_REGISTERS_T *) pXllpLCD->LCDC;
    pLCDRegs->lccr6 = 0;
    pLCDRegs->fbr0 = 0;

    pLCDRegs->lccr1 |= XLLP_LCCR1_HSW(timing->HSW) 
        | XLLP_LCCR1_ELW(timing->ELW) 
        | XLLP_LCCR1_BLW(timing->BLW);

    pLCDRegs->lccr2 |= XLLP_LCCR2_VSW(timing->VSW) 
        | XLLP_LCCR2_EFW(timing->EFW)  
        | XLLP_LCCR2_BFW(timing->BFW);

    pLCDRegs->lccr3 |= XLLP_LCCR3_OEP(polarity->OEP)
        | XLLP_LCCR3_PCP(polarity->PCP)
        | XLLP_LCCR3_HSP(polarity->HSP) 
        | XLLP_LCCR3_VSP(polarity->HSP);
            
    TFTUpdateClock(pXllpLCD, HSS_AUTO_DETECT);

    // Load the contents of FDADR0 with the physical address of this frame descriptor
    pLCDRegs->fdadr0 = XLLP_FDADR_DESCADDR(pXllpLCD->frameDescriptorCh0fd1->FDADR);
    pLCDRegs->lccr0 |= XLLP_LCCR0_ENB;
}

static const XLLP_UINT32_T lcd_mfp_list[] = {
    XLLP_MFP_L_DD_8_OFFSET,
    XLLP_MFP_L_DD_9_OFFSET,
    XLLP_MFP_L_DD_10_OFFSET,
    XLLP_MFP_L_DD_11_OFFSET,
    XLLP_MFP_L_DD_12_OFFSET,
    XLLP_MFP_L_DD_13_OFFSET,
    XLLP_MFP_L_DD_14_OFFSET,
    XLLP_MFP_L_DD_15_OFFSET,
    XLLP_MFP_L_DD_16_OFFSET,
    XLLP_MFP_L_DD_17_OFFSET,
    XLLP_MFP_L_DD_0_OFFSET,
    XLLP_MFP_L_DD_1_OFFSET,
    XLLP_MFP_L_DD_2_OFFSET,
    XLLP_MFP_L_DD_3_OFFSET,
    XLLP_MFP_L_DD_4_OFFSET,
    XLLP_MFP_L_DD_5_OFFSET,
    XLLP_MFP_L_DD_6_OFFSET,
    XLLP_MFP_L_DD_7_OFFSET,
    XLLP_MFP_L_FCLK_OFFSET,
    XLLP_MFP_L_LCLK_OFFSET,
    XLLP_MFP_L_PCLK_OFFSET,
    XLLP_MFP_L_BIAS_OFFSET,

//    XLLP_MFP_BACKLIGHT_PWM_OFFSET,

    XLLP_MFP_PIN_EOLIST_MARKER
};
static const XLLP_MFP_ALT_FN_T lcd_af_list[] = {
    XLLP_MFP_L_DD_8_AF,         //L= #define XLLP_MFP_L_DD_8_AF, P= (XLLP_MFP_PIN_GPIO63_AF_L_DD_8)
    XLLP_MFP_L_DD_9_AF,
    XLLP_MFP_L_DD_10_AF,
    XLLP_MFP_L_DD_11_AF,
    XLLP_MFP_L_DD_12_AF,
    XLLP_MFP_L_DD_13_AF,
    XLLP_MFP_L_DD_14_AF,
    XLLP_MFP_L_DD_15_AF,
    XLLP_MFP_L_DD_16_AF,
    XLLP_MFP_L_DD_17_AF,
    XLLP_MFP_L_DD_0_AF,
    XLLP_MFP_L_DD_1_AF,
    XLLP_MFP_L_DD_2_AF,
    XLLP_MFP_L_DD_3_AF,
    XLLP_MFP_L_DD_4_AF,
    XLLP_MFP_L_DD_5_AF,
    XLLP_MFP_L_DD_6_AF,
    XLLP_MFP_L_DD_7_AF,
    XLLP_MFP_L_FCLK_AF,
    XLLP_MFP_L_LCLK_AF,
    XLLP_MFP_L_PCLK_AF,
    XLLP_MFP_L_BIAS_AF,

//    XLLP_MFP_ALT_FN_0
};
static const XLLP_MFP_DRIVE_STRENGTH_T lcd_ds_list[] = {
    XLLP_MFP_DS_01X,            //0
    XLLP_MFP_DS_01X,
    XLLP_MFP_DS_01X,
    XLLP_MFP_DS_01X,
    XLLP_MFP_DS_01X,
    XLLP_MFP_DS_01X,            //5
    XLLP_MFP_DS_01X,
    XLLP_MFP_DS_01X,
    XLLP_MFP_DS_01X,
    XLLP_MFP_DS_01X,
    XLLP_MFP_DS_01X,            //10
    XLLP_MFP_DS_01X,
    XLLP_MFP_DS_01X,
    XLLP_MFP_DS_01X,
    XLLP_MFP_DS_01X,
    XLLP_MFP_DS_01X,            //15
    XLLP_MFP_DS_01X,
    XLLP_MFP_DS_01X,
    XLLP_MFP_DS_01X,
    XLLP_MFP_DS_01X,
    XLLP_MFP_DS_04X,            //20
    XLLP_MFP_DS_01X,            //21
//    XLLP_MFP_DS_01X,            //22    
};
static const XLLP_MFP_LPM_OUTPUT_T lcd_lpm_output_list[] = {
    XLLP_MFP_LPMO_PULL_LOW,     //  0
    XLLP_MFP_LPMO_PULL_LOW,     //  
    XLLP_MFP_LPMO_PULL_LOW,     //  
    XLLP_MFP_LPMO_PULL_LOW,     //  
    XLLP_MFP_LPMO_PULL_LOW,     //  
    XLLP_MFP_LPMO_PULL_LOW,     //  5
    XLLP_MFP_LPMO_PULL_LOW,     //   
    XLLP_MFP_LPMO_PULL_LOW,     //   
    XLLP_MFP_LPMO_PULL_LOW,     //   
    XLLP_MFP_LPMO_PULL_LOW,     //   
    XLLP_MFP_LPMO_PULL_LOW,     //  10
    XLLP_MFP_LPMO_PULL_LOW,     //   
    XLLP_MFP_LPMO_PULL_LOW,     //   
    XLLP_MFP_LPMO_PULL_LOW,     //   
    XLLP_MFP_LPMO_PULL_LOW,     //   
    XLLP_MFP_LPMO_PULL_LOW,     //  15
    XLLP_MFP_LPMO_PULL_LOW,     //   
    XLLP_MFP_LPMO_PULL_LOW,     //   
    XLLP_MFP_LPMO_PULL_LOW,     //   
    XLLP_MFP_LPMO_PULL_LOW,     //   
    XLLP_MFP_LPMO_PULL_LOW,     //  20
    XLLP_MFP_LPMO_PULL_LOW,     //  21 
//    XLLP_MFP_LPMO_PULL_LOW      //  22 
};

static XLLP_STATUS_T init_pins(P_XLLP_LCD_T pXllpLCD)
{
    XLLP_STATUS_T status = 0;

    // Lock MFPR block until finished with all MFPRs.
    // Don't forget to unlock for early returns, too.
    
    
    // Initialize the MFP registers for proper LCD Controller operation
    status =  XllpMfpSetAfDs_List((P_XLLP_VUINT32_T)(pXllpLCD->MFP), 
                                  (P_XLLP_UINT32_T)lcd_mfp_list, 
                                  (P_XLLP_MFP_ALT_FN_T)lcd_af_list, 
                                  (P_XLLP_MFP_DRIVE_STRENGTH_T)lcd_ds_list);
    if (status != XLLP_STATUS_SUCCESS)
        goto err1;
        
    status = XllpMfpConfigureLpmOutputLevel_List((P_XLLP_VUINT32_T)(pXllpLCD->MFP),
                                                 (P_XLLP_UINT32_T)lcd_mfp_list,
                                                 (P_XLLP_MFP_LPM_OUTPUT_T)lcd_lpm_output_list);
    if (status != XLLP_STATUS_SUCCESS)
        goto err1;

    status = XllpMfpActivatePullUpDown_List((P_XLLP_VUINT32_T)(pXllpLCD->MFP),
                                            (P_XLLP_UINT32_T)lcd_mfp_list,
                                             XLLP_OFF);
    if (status != XLLP_STATUS_SUCCESS)
        goto err1;
err1:
    return status;	    
}


XLLP_MINI_LCD_T xllp_mini_lcd;
volatile UINT32* LCD_FDADR0;
volatile XLLP_LCD_REGISTERS_T* LCDRegs;
#define MINI_LCD_OFFSET 0x10000
#define MINI_LCD_FRAME_PA (MONAHANS_BASE_REG_PA_IMSTORAGE + MINI_LCD_OFFSET)
#define IM_SIZE 0x40000 	// in Monahans L, sram size is 256k

void InitMiniLCDC(void)
{
    LCDRegs = (XLLP_LCD_REGISTERS_T*) OALPAtoVA(MONAHANS_BASE_REG_PA_LCD, FALSE);
    LCD_FDADR0 = &(LCDRegs->fdadr0);

    xllp_mini_lcd.MFP = (XLLP_VUINT32_T *) OALPAtoVA(MONAHANS_BASE_REG_PA_MFP, FALSE);
    xllp_mini_lcd.CLKMan = (XLLP_VUINT32_T *) OALPAtoVA(MONAHANS_BASE_REG_PA_CLKMGR, FALSE);
    xllp_mini_lcd.MLCD = (XLLP_VUINT32_T *) OALPAtoVA(MONAHANS_BASE_REG_PA_MLCD, FALSE);
    xllp_mini_lcd.SRAM_phys = (XLLP_UINT32_T) MINI_LCD_FRAME_PA;
    xllp_mini_lcd.SRAM_virt = (XLLP_UINT32_T) OALPAtoVA(MINI_LCD_FRAME_PA, FALSE);
    xllp_mini_lcd.SRAMQuota = IM_SIZE - MINI_LCD_OFFSET;

    //XllpMiniLCDInit776C(&xllp_mini_lcd);
}


void InitLCDC(void)
{
    volatile int x=0;

    unsigned short *cmd_buf_phy = NULL;
    XLLP_LCD_T XllpLCD;
    volatile unsigned int *v_pOSTRegs = (volatile unsigned int *)OALPAtoVA(MONAHANS_BASE_REG_PA_OST, FALSE);
    volatile XLLP_LCD_REGISTERS_T *v_pLcdRegs = (volatile XLLP_LCD_REGISTERS_T *)OALPAtoVA(MONAHANS_BASE_REG_PA_LCD, FALSE);
    volatile XLLP_CLKMGR_T *v_pClkRegs = (volatile XLLP_CLKMGR_T *)OALPAtoVA(MONAHANS_BASE_REG_PA_CLKMGR, FALSE);
    volatile XLLP_GPIO_T *v_pGPIORegs = (volatile XLLP_GPIO_T *)OALPAtoVA(MONAHANS_BASE_REG_PA_GPIO, FALSE);
    volatile unsigned int *v_pMFPRegs = (volatile unsigned int*)OALPAtoVA(MONAHANS_BASE_REG_PA_MFP, FALSE);

    RETAILMSG(1, (TEXT("+DispDrvrInitialize")));

    alloc_map_mem();
    DrawBuffer();

    XllpLCD.GPIO = (XLLP_VUINT32_T *) v_pGPIORegs;
    XllpLCD.MFP = (XLLP_VUINT32_T *) v_pMFPRegs;
    XllpLCD.CLKMan = (XLLP_VUINT32_T *) v_pClkRegs; 
    XllpLCD.LCDC = (XLLP_VUINT32_T *) v_pLcdRegs;
    XllpLCD.OST = (XLLP_VUINT32_T *) v_pOSTRegs;
    XllpLCD.BPP = BPP_16;
    XllpLCD.PixelDataFormat = PDFOR_00; //with overlays enabled use PDFOR_11 for 16bpp

    XllpLCD._COMMAND_BUFFER_BASE_PHYSICAL = (XLLP_UINT32_T)cmd_buf_phy;
    XllpLCD.cmd_buf = cmd_buf;
    XllpLCD.cmd_max = SMART_CMD_BUF_SIZE / sizeof(*cmd_buf);

    XllpLCD._DMA_CHANNEL_0_FRAME_DESCRIPTOR_BASE_PHYSICAL = (XLLP_UINT32_T)&dma_descriptors_phy[0];    
    XllpLCD._DMA_CHANNEL_0_ALT_FRAME_DESCRIPTOR_BASE_PHYSICAL = (XLLP_UINT32_T)(&dma_descriptors_phy[DMA_CHANNEL_UNUSED]);
    XllpLCD.frameDescriptorCh0fd1 = &dma_descriptors[0];
    XllpLCD.frameDescriptorCh0fd2 = &dma_descriptors[DMA_CHANNEL_UNUSED];

    XllpLCD._DMA_CHANNEL_2_Y_FRAME_DESCRIPTOR_BASE_PHYSICAL = (XLLP_UINT32_T)&dma_descriptors_phy[2];
    XllpLCD.frameDescriptorCh2_YCbCr_Y = &dma_descriptors[2];

    XllpLCD._DMA_CHANNEL_3_Cb_FRAME_DESCRIPTOR_BASE_PHYSICAL = (XLLP_UINT32_T)&dma_descriptors_phy[3];
    XllpLCD.frameDescriptorCh3_YCbCr_Cb = &dma_descriptors[3];

    XllpLCD._DMA_CHANNEL_4_Cr_FRAME_DESCRIPTOR_BASE_PHYSICAL = (XLLP_UINT32_T)&dma_descriptors_phy[4];
    XllpLCD.frameDescriptorCh4_YCbCr_Cr = &dma_descriptors[4];

    XllpLCD._DMA_CHANNEL_6_COMMAND_DESCRIPTOR_BASE_PHYSICAL = (XLLP_UINT32_T)&dma_descriptors_phy[6];
    XllpLCD.frameDescriptorCh6_command = &dma_descriptors[6];

    XllpLCD._FRAME_BUFFER_BASE_PHYSICAL = BMAN_FB_PHY_ADX;

    //panel_select();
    NKDbgPrintfW(TEXT("using lcd panel 766C(QVGA).\r\n"));
    //XllpLCDInit776C(&XllpLCD);

    XllpLCDInitTopoly(&XllpLCD, MODE_VGA);
    XllpLCD.panel->tft.init_pins(&XllpLCD);


    XllpLCD.overlay2 = NULL;
    // just set up AF's.  Unreal how many levels of indirection are here for pins.
    //
    init_pins(&XllpLCD);

    //while(x==0);

    LCDInitDMA(&XllpLCD);
    LocalTFTInitController(&XllpLCD);

    if (XllpLCD.panel->tft.post_init) 
        XllpLCD.panel->tft.post_init(&XllpLCD);
    
    RETAILMSG(1, (TEXT("-DispDrvrInitialize")));
}


// glob
DWORD MenuStandby(MENU_ITEM *p);
static gbMiniLCD=FALSE;

DWORD MenuLCD(MENU_ITEM *p)
{
	RETAILMSG(1, (TEXT("+MenuLCD \r\n")));
	InitLCDC();
	RETAILMSG(1, (TEXT("-MenuLCD\r\n")));
	return 0;
}

DWORD MenuMiniLCD(MENU_ITEM *p)
{
    // Assumes LCD is already going
    // for first test, just bang in and out of mode...
    //
    gbMiniLCD=TRUE;

    InitLCDC();
    InitMiniLCDC();

    lcd_reg_save();
    mini_lcd_on();

    MenuStandby(p);     // not using p...

    mini_lcd_off();
    lcd_reg_restore();

    return 0;
}

DWORD MenuStandby(MENU_ITEM *p)
{
#if 0
	// config to wake on something cool
    //
    volatile XLLP_PM_BPMU_T *pBPMU   = (volatile XLLP_PM_BPMU_T *)OALPAtoVA(MONAHANS_BASE_REG_PA_BPMU, FALSE);
    volatile XLLP_OST_T     *pOSTReg = (volatile XLLP_OST_T *)OALPAtoVA(MONAHANS_BASE_REG_PA_OST, FALSE);
    volatile UINT32     *pHEXLED = (volatile UINT32 *)OALPAtoVA(0x14200000, FALSE);
    IPMD1D2StructTest IpmD1D2Struct;
    UINT32 cnt, stall, roll, inc, counter=0;

    IpmD1D2Struct.ReloStartAdx = (UINT32)OALPAtoVA(0x5c028000, FALSE);
    IpmD1D2Struct.DMCBaseAdx   = (UINT32)OALPAtoVA(MONAHANS_BASE_REG_PA_DMEMC, FALSE);
    IpmD1D2Struct.ReloSPAdx    = (UINT32)OALPAtoVA(0x5c040000, FALSE);
    if(gbMiniLCD)
        IpmD1D2Struct.CP14Val      = 2;     // D1
    else
        IpmD1D2Struct.CP14Val      = 3;     // D2

    cnt=roll=inc=0;
    // Set up OST
    //
    pOSTReg->omcr4 = 0x42;      // timer resolution = ~ 1ms (for OS use '1' for 1/32768th of a sec)
    pOSTReg->oscr4 = 1;     // must start with non-0 value :(

    while(1)
    {
    

            // Perform LV-specific power opts that need to find their way into the BSP
            //
            XLLP_VUINT32_T * pOST, * pI2C, *pMFP;
            XLLP_UINT8_T buf[2];
            XLLP_STATUS_T ret;
            char pMfpRMDb[XLLP_MFP_RM_DB_SIZE];

            // (PMIC) Micco-specific
            pOST= (XLLP_VUINT32_T *) OALPAtoVA(MONAHANS_BASE_REG_PA_OST, FALSE);
            pI2C= (XLLP_VUINT32_T *) OALPAtoVA(MONAHANS_BASE_REG_PA_I2C, FALSE);
            pMFP= (XLLP_VUINT32_T *) OALPAtoVA(MONAHANS_BASE_REG_PA_MFP, FALSE);

            pMFP= (XLLP_VUINT32_T *) OALPAtoVA(0x40e100b4, FALSE);
            *pMFP++ = 0xA0C0;
            *pMFP++ = 0xA0C0;
            *pMFP   = 0xA0C0;
            pMFP= (XLLP_VUINT32_T *) OALPAtoVA(0x40e102c4, FALSE);
            *pMFP++ = 0x0141;
            *pMFP   = 0x0141;
            pMFP= (XLLP_VUINT32_T *) OALPAtoVA(0x40e10674, FALSE);
            *pMFP++ = 0xC0C0;
            *pMFP   = 0xC0C0;
            pMFP= (XLLP_VUINT32_T *) OALPAtoVA(0x40e1052c, FALSE);
            *pMFP++ = 0x01C1;
            *pMFP++ = 0x01C1;
            *pMFP++ = 0x01C1;
            *pMFP   = 0x01C1;
            pMFP= (XLLP_VUINT32_T *) OALPAtoVA(MONAHANS_BASE_REG_PA_MFP, FALSE);       // restore

//            DumpMFPAutoLv();            // enable to dump Lv MFP.
            msWait(10); 


            if (XLLP_STATUS_SUCCESS!=XllpMfpResourceManagerInit(pMfpRMDb))
            {
                EdbgOutputDebugString("[failed to XllpMfpResourceManagerInit()!\r\n");
            }
            XllpI2cInit((P_XLLP_I2C_T)pI2C, (P_XLLP_VUINT32_T) pMFP, (P_XLLP_MFP_RM_DB_ID_T )pMfpRMDb, 0);

            //
            // Things to turn off:
            //  x17
            buf[0]=0xC7;
            ret=XllpPmMiccoWrite ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x17, buf[0]);       // ..,..,reg, val
            if(XLLP_STATUS_SUCCESS!= ret)
                EdbgOutputDebugString("write Micco returned 0x%x, value:%x\r\n", (unsigned int) ret,(unsigned int)buf[0]);

            //  x18
            buf[0]=0x03;
            ret=XllpPmMiccoWrite ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x18, buf[0]);       // ..,..,reg, val
            if(XLLP_STATUS_SUCCESS!= ret)
                EdbgOutputDebugString("write Micco returned 0x%x, value:%x\r\n", (unsigned int) ret,(unsigned int)buf[0]);

            //  x97
            buf[0]=0x10;
            ret=XllpPmMiccoWrite ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x97, buf[0]);       // ..,..,reg, val
            if(XLLP_STATUS_SUCCESS!= ret)
                EdbgOutputDebugString("write Micco returned 0x%x, value:%x\r\n", (unsigned int) ret,(unsigned int)buf[0]);

            //  x98
            buf[0]=0x0C;
            ret=XllpPmMiccoWrite ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x98, buf[0]);       // ..,..,reg, val
            if(XLLP_STATUS_SUCCESS!= ret)
                EdbgOutputDebugString("write Micco returned 0x%x, value:%x\r\n", (unsigned int) ret,(unsigned int)buf[0]);

            //  x22: 5V debug domain shut down (-ish for <G4 rev LTK mainboards)
            buf[0]=0x80;
            ret=XllpPmMiccoWrite ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x22, buf[0]);       // ..,..,reg, val
            if(XLLP_STATUS_SUCCESS!= ret)
                EdbgOutputDebugString("write Micco returned 0x%x, value:%x\r\n", (unsigned int) ret,(unsigned int)buf[0]);

            //  x23: 3.2V debug domain
            buf[0]=0x80;
            ret=XllpPmMiccoWrite ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x23, buf[0]);       // ..,..,reg, val
            if(XLLP_STATUS_SUCCESS!= ret)
                EdbgOutputDebugString("write Micco returned 0x%x, value:%x\r\n", (unsigned int) ret,(unsigned int)buf[0]);

            // Put Buck2 into Pulse-skip mode while in D2.  Take out manually upon resume.
            //
            ret=XllpPmMiccoRead ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x15, buf);
            buf[0] = (buf[0] | (0x80 | 0x40));          // GO|BUCK2SLEEP    (manually forcing into PSM)
            //buf[0] = (buf[0] | (0x80 | 0x20));          // GO|AUTO_SLEEP      (assumes will enter PSM by itself... not convinced is bulletproof)
            ret=XllpPmMiccoWrite ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x15, buf[0]);       // ..,..,reg, val
            if(XLLP_STATUS_SUCCESS!= ret)
                EdbgOutputDebugString("write Micco returned 0x%x, value:%x\r\n", (unsigned int) ret,(unsigned int)buf[0]);

            ret=XllpPmMiccoRead ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x15, buf);
            EdbgOutputDebugString("Read Micco returned 0x%x for BUCK2 reg\r\n", buf[0]);
            msWait(1000); 



        *pHEXLED = counter++;

        pBPMU->ascr = pBPMU->ascr;      // clear any status
        pBPMU->arsr = pBPMU->arsr;
       
        pBPMU->ad2r = 0x13F;     // all banks maintained for now, plus the L2 bit
        pBPMU->ad1r = 0x13F;     // all banks ON for now, plus the L2 bit
       
        // Set up wake event(s)
        //
        pBPMU->ad2d0sr = WAKE_MASK;     // sticky status
        pBPMU->ad2d0er = WAKE_MASK;     // set wake source
        pBPMU->ad1d0sr = WAKE_MASK;     // sticky status
        pBPMU->ad1d0er = WAKE_MASK;     // set wake source

        // Prep OST
        //
        inc += 10;
        if(inc >= (1000*60))    // let it grow by 10ms until it reaches 1 minute
            inc=0;

        pOSTReg->ossr = 0xFFF;          // clear any stati
        pOSTReg->oier = 0x10;           // OSCR4 match updates status
        pOSTReg->osmr4 = pOSTReg->oscr4 + inc;      // 50ms till match

        /* ; Input Params:  
        ;        R0: start addy of relocation section of IM
        ;		 R1: DMC base addy
        ;		 R2: IM-resident SP
        */
        //EnterD2(OALPAtoVA(0x5c028000, FALSE), OALPAtoVA(MONAHANS_BASE_REG_PA_DMEMC, FALSE), OALPAtoVA(0x5c040000, FALSE));
        EnterLPM(&IpmD1D2Struct);


            XLLP_VUINT32_T * pOST, * pI2C;
            XLLP_UINT8_T buf[2];
            XLLP_STATUS_T ret;

            // (PMIC) Micco-specific
            pOST= (XLLP_VUINT32_T *) OALPAtoVA(MONAHANS_BASE_REG_PA_OST, FALSE);
            pI2C= (XLLP_VUINT32_T *) OALPAtoVA(MONAHANS_BASE_REG_PA_I2C, FALSE);

            // re-enable 5 and 3.2V domains
            //
            buf[0] = 0;
            ret=XllpPmMiccoWrite ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x22, buf[0]);       // ..,..,reg, val
            if(XLLP_STATUS_SUCCESS!= ret)
                EdbgOutputDebugString("write Micco returned 0x%x, value:%x\r\n", (unsigned int) ret,(unsigned int)buf[0]);

            buf[0] = 0;
            ret=XllpPmMiccoWrite ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x23, buf[0]);       // ..,..,reg, val
            if(XLLP_STATUS_SUCCESS!= ret)
                EdbgOutputDebugString("write Micco returned 0x%x, value:%x\r\n", (unsigned int) ret,(unsigned int)buf[0]);

            // Kick BUCK2 out of PSM
            //
            ret=XllpPmMiccoRead ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x15, buf);
            buf[0] = ((buf[0] & ~0x40) | 0x80);
            ret=XllpPmMiccoWrite ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x15, buf[0]);       // ..,..,reg, val
            if(XLLP_STATUS_SUCCESS!= ret)
                EdbgOutputDebugString("write Micco returned 0x%x, value:%x\r\n", (unsigned int) ret,(unsigned int)buf[0]);

            ret=XllpPmMiccoRead ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x15, buf);
            EdbgOutputDebugString("Read Micco returned 0x%x for BUCK2 reg\r\n", buf[0]);
            msWait(100); 

        if(++cnt == 0xFFFFFFFF)
            roll++;

        EdbgOutputDebugString("%x %x\r\n", roll,cnt);

        stall = pOSTReg->oscr4 + 50;        // let uart drain our count (50ms)
        while(pOSTReg->oscr4 < stall);
    }
#endif

    return 0;
}

DWORD MenuSleep(MENU_ITEM *p)
{
    XLLP_VUINT32_T * pOST, * pI2C, *pMFP;
    XLLP_UINT8_T buf[2];
    XLLP_STATUS_T ret;
    char pMfpRMDb[XLLP_MFP_RM_DB_SIZE];


    // remove the sram workaround in mobmV2 for special power ECO boards
    if (g_pBSPArgs->SpecialECO)
    {
        pOST= (XLLP_VUINT32_T *) OALPAtoVA(MONAHANS_BASE_REG_PA_OST, FALSE);
        pI2C= (XLLP_VUINT32_T *) OALPAtoVA(MONAHANS_BASE_REG_PA_I2C, FALSE);
        pMFP= (XLLP_VUINT32_T *) OALPAtoVA(MONAHANS_BASE_REG_PA_MFP, FALSE);

        EdbgOutputDebugString ( "\r\nAllowing Micco to shut down VCC_APPS and VCC_SRAM in D3...\r\n");

        if (XLLP_STATUS_SUCCESS!=XllpMfpResourceManagerInit(pMfpRMDb))
        {
            EdbgOutputDebugString("[failed to XllpMfpResourceManagerInit()!\r\n");
        }
        XllpI2cInit((P_XLLP_I2C_T)pI2C, (P_XLLP_VUINT32_T) pMFP, (P_XLLP_MFP_RM_DB_ID_T )pMfpRMDb, 0);
        ret=XllpPmMiccoRead ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x1A, buf);
        if(XLLP_STATUS_SUCCESS!= ret)
            EdbgOutputDebugString("Before write Micco returned 0x%x, value:%x\r\n", (unsigned int) ret,(unsigned int)buf[0]);
        buf[0]=0;
        ret=XllpPmMiccoWrite ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x1A, buf[0]);
        if(XLLP_STATUS_SUCCESS!= ret)
            EdbgOutputDebugString("write Micco returned 0x%x, value:%x\r\n", (unsigned int) ret,(unsigned int)buf[0]);
        ret=XllpPmMiccoRead ((P_XLLP_I2C_T) pI2C, (XLLP_OST_T *)pOST, 0x1A, buf);
        if(XLLP_STATUS_SUCCESS!= ret)
            EdbgOutputDebugString("double check read Micco returned 0x%x, value:%x\r\n", (unsigned int) ret,(unsigned int)buf[0]);

    }

    EdbgOutputDebugString("Call OEMPowerOff Press SW6 to return back\r\n ");
    OEMPowerOff();
    FMD_PowerUp();
    return 0;
}

typedef struct
{
    volatile DWORD *pACCR;
    volatile DWORD *pMDCNFG;
    volatile DWORD *pOSCR0;
    volatile DWORD *pGPSR0;
    BOOL	 bDisablePLL;
} D0CS_STRUCT;

extern void Enter_D0CS(D0CS_STRUCT *pBuf);
extern void Exit_D0CS(volatile void *, volatile void *);

#define MEMCLKCFG_DFCLKDIV_1	(0x1 << 16)
#define MEMCLKCFG_DFCLKDIV_2	(0x2 << 16)
#define MEMCLKCFG_DFCLKDIV_4	(0x3 << 16)

#define MEMCLKCFG_EMPICLKDIV_1	(0x1)
#define MEMCLKCFG_EMPICLKDIV_2	(0x2)
#define MEMCLKCFG_EMPICLKDIV_4	(0x3)


/* ***************************************************************************************************** */
/* ***************************************************************************************************** */
void HandleD0CS(BOOL EnterD0CS)
{
    volatile XLLP_CLKMGR_T *pCLKS 		= (volatile XLLP_CLKMGR_T *)OALPAtoVA(MONAHANS_BASE_REG_PA_CLKMGR, FALSE);
    volatile XLLP_SMEM_REGISTER_T *pSMC = (volatile XLLP_SMEM_REGISTER_T *)OALPAtoVA(MONAHANS_BASE_REG_PA_SMEMC, FALSE);
    volatile DWORD *pMDCNFG 			= (volatile DWORD *)OALPAtoVA((MONAHANS_BASE_REG_PA_DMEMC + 0), FALSE);
    volatile DWORD *pOSCR0 				= (volatile DWORD *)OALPAtoVA((MONAHANS_BASE_REG_PA_OST + 0x10), FALSE);
    volatile DWORD *pGPSR0 				= (volatile DWORD *)OALPAtoVA((MONAHANS_BASE_REG_PA_GPIO + 0x18), FALSE);

    //volatile XLLP_LCD_REGISTERS_T *pLCDC = (volatile XLLP_LCD_REGISTERS_T *)OALPAtoVA(MONAHANS_BASE_REG_PA_LCD, FALSE);
    XLLP_PM_FV_INFO nextOP;

    // OP[0]: D0CS
    nextOP.xl = 0;      							// will not actually adjust this
    nextOP.xn = 0;      							// will not actually adjust this
    nextOP.vcc_core = 1000;  						// *NOTE: voltages same as 104; no need to VCS if coming from 104*
    nextOP.vcc_sram = 1100;						// *NOTE: voltages same as 104; no need to VCS if coming from 104*                                                       
    nextOP.smcfs = 0;   							// will not actually adjust this
    nextOP.sflfs = 0;   							// will not actually adjust this
    nextOP.hss = 0;         						// will not actually adjust this
    nextOP.dmcfs = 0;								// will not actually adjust this
    nextOP.df_clk = XLLP_PM_DF_CLKDIV_1;     		// this will get adjusted   
    nextOP.empi_clk = XLLP_PM_EMPI_CLKDIV_1;		// this will get adjusted
    nextOP.d0cs = 1;		 

    switch (EnterD0CS) 
    {
        case TRUE:
            {
                D0CS_STRUCT d0cs_struct;

                //RETAILMSG(1, (TEXT("+HandleD0CS-Enter\r\n")));

                if(pCLKS->acsr & XLLP_ACSR_RO_S)
                    return;										// already in d0cs.

                // Handle MEMCLKCFG stuff.  There is no way we can be going faster, but may be the same.
                // 
                if((pSMC->memclkcfg & 0x7) > nextOP.empi_clk)
                {

                    // program MEMCLKCFG
                    //
                    pSMC->memclkcfg = ((pSMC->memclkcfg & 0x00070000) | (nextOP.empi_clk & 0x7)); 
                    while((pSMC->memclkcfg & 0x7) != (nextOP.empi_clk & 0x7));
   
                }

                if( ((pSMC->memclkcfg >> 16) & 0x7) > (nextOP.df_clk & 0x7))
                {

                    // program MEMCLKCFG, preserve empi_clkdiv
                    //
                    pSMC->memclkcfg = ((pSMC->memclkcfg & 0x00000007) | ((nextOP.df_clk & 0x7) << 16)); 
                    while((pSMC->memclkcfg & 0x00070000) != ((nextOP.df_clk & 0x7) << 16));
    
                }

                // To ensure sidecar CKEN is on before entering.  Can re-disable once in.  Need to either push into preamble, or have driver do it.
                //pCLKS->d0cken_b &= 0xFFFFFDFF;

                //  ?DRI?: NOP since refresh logic now uses fixed 13MHz clock.
                //
                d0cs_struct.pACCR = (DWORD *)(pCLKS + 0);			// accr is 0 offset 
                d0cs_struct.pMDCNFG = pMDCNFG;				// mdcnfg is 0 offset
                d0cs_struct.pOSCR0 = pOSCR0;				// OSCR0 is 0x10 offset
                d0cs_struct.pGPSR0 = pGPSR0;				// GPSR0 is 0x18 offset (USED FOR DEBUG ONLY)
                d0cs_struct.bDisablePLL = TRUE;				// always disable the PLLs when using D0CS
        
                /*while((pLCDC->lcsr0 & XLLP_LCSR0_EOF0) != XLLP_LCSR0_EOF0);
                pLCDC->lcsr0 = XLLP_LCSR0_EOF0;  */

                Enter_D0CS(&d0cs_struct);					// *Note: any ints_on/off done in the ASM 


                break;
            }

        case FALSE:
            {

                // Max SMC.MEMCFG
                pSMC->memclkcfg = (MEMCLKCFG_DFCLKDIV_4 | MEMCLKCFG_EMPICLKDIV_4);

                Exit_D0CS((DWORD *)(pCLKS +0), pMDCNFG);


                break;
            }
    }

}

static void Wait(UINT32 microSeconds)
{
#if MINI_EBOOT
    volatile UINT32 *TimerOSCRAddress= (volatile UINT32 *)(MONAHANS_BASE_REG_PA_OST + OSCR_OFFSET);
#else
    volatile UINT32 *TimerOSCRAddress= (volatile UINT32 *) OALPAtoVA((MONAHANS_BASE_REG_PA_OST + OSCR_OFFSET), FALSE);
#endif

    UINT32 Value, Time;

    Time   = *TimerOSCRAddress;
    Value = Time + (microSeconds * 4);
    if (Value < Time)
    {  // test for wrap.
        while (Time < *TimerOSCRAddress);
    }
    while (*TimerOSCRAddress <= Value);

}

static void msWait(UINT32 msVal) 
{
    Wait(msVal*1000);
}



DWORD MenuSleepD0CSTest(MENU_ITEM *p)
{
    int i = 0;
    BOOL result = TRUE;
    UINT32 bogusmips=0 , ACSR =0;

    volatile DWORD *pACSR = (volatile DWORD *)(OALPAtoVA(0x41340004, FALSE)); // ACSR
    volatile UINT32 *pOSCR0= (volatile UINT32 *)(OALPAtoVA(0x40A00010, FALSE)); // OSCR for benchies

    volatile DWORD *pRTCReg = (volatile DWORD *)(OALPAtoVA(0x40900000, FALSE)); // RTC for wakeup
    XLLP_PM_BPMU_T  *pBPMUReg=NULL;

    pBPMUReg = OALPAtoVA(MONAHANS_BASE_REG_PA_BPMU, FALSE);


    WriteSimpleDDR();    
    for (i= 0; i != -1; i ++)
    {
        int loop =0;   
        int count = 0;
        volatile int x = 0;
        UINT32 oscrstart = 0;
        UINT32 oscrend = 0;

        EdbgOutputDebugString("\r\nSleep  #%d \r\n", i);
        EdbgOutputDebugString("Sleeping... \r\n");
        XllpRtcConfigureIncreaseAlarm( (P_XLLP_RTC_T)pRTCReg, 1);
        OEMPowerOff();
        #ifdef BogusD0CSMIPSTEST  
        msWait(100);
        EdbgOutputDebugString("Enter D0CS  #%d\r\n", i);
        msWait(100);
        HandleD0CS(TRUE);

        EdbgOutputDebugString("Huh? ACSR = %x \r\n", *pACSR);
        ACSR = *pACSR;

        oscrstart = *pOSCR0;

        for (loop = 1; loop <1000000; loop ++)
        {
            x++;
        }
        oscrend = *pOSCR0;
        /*
        oscrend = *pOSCR0 + 325000; //100 ms
        while (*pOSCR0 < oscrend) 
        {
            count++;
        } */
        count = oscrend - oscrstart;
        EdbgOutputDebugString("Bogus MIPS time to do 1 mil stuf = %d\r\n", count);
        bogusmips = count;


        msWait(100);
        HandleD0CS(FALSE);
        msWait(100);
        EdbgOutputDebugString("Huh? ACSR = %x \r\n", ACSR);
        EdbgOutputDebugString("Bogus MIPS time to do 1 mil stuf = %d\r\n", count);
        #endif

        //  FMD_PowerUp();

        msWait(100);
        EdbgOutputDebugString("Enter D0CS  #%d\r\n", i);
        msWait(100);
        HandleD0CS(TRUE);

        EdbgOutputDebugString("Huh? ACSR = %x \r\n", *pACSR);
        result = WriteReadSilent();
        msWait(100);
        HandleD0CS(FALSE);
        msWait(100);

        if (!result)
        {
            EdbgOutputDebugString("Exit D0CS - FAIL DDR!\r\n ");
            break;
        }
        else
            EdbgOutputDebugString("Exit D0CS - PASS DDR! ACSR = %x\r\n ", *pACSR);

        //EdbgOutputDebugString("Exit D0CS\r\n ");


    }
    return 0;
}

DWORD MenuLoadOS(MENU_ITEM *p)
{
    LoadImageDDR((BOOL) p->pParam1);
    return 0;
}

DWORD WriteRdDDR(MENU_ITEM *p)
{
    WriteReadDDR((UINT32)p->pParam1);;
    return 0;
}

typedef VOID (*VOIDF)(void);

DWORD CallP1VOID(MENU_ITEM *p)
{
    if(p->pParam1)
        ((VOIDF)(p->pParam1))();
    return 0;
}
BOOL StoreBSPCFG(BSP_ARGS *pArgs);

DWORD MenuFormat(MENU_ITEM *p)
{
    int startBlock, endBlock;
    EdbgOutputDebugString ("Enter start block\r\n");
    if (g_FlashInfo.wDataBytesPerSector == LB_BYTES_PER_PAGE )
    {
        startBlock = GetBlockAddr(LB_FLASH_BLOCK_OS_START, (g_FlashInfo.dwNumBlocks - 1));
        EdbgOutputDebugString ("\r\n");			
        EdbgOutputDebugString ("Enter end block\r\n");
        endBlock = GetBlockAddr(LB_FLASH_BLOCK_OS_START, (g_FlashInfo.dwNumBlocks - 1));
    }
    else
    {
        startBlock = GetBlockAddr(FLASH_BLOCK_OS_START, (g_FlashInfo.dwNumBlocks - 1));
        EdbgOutputDebugString ("\r\n");			
        EdbgOutputDebugString ("Enter end block\r\n");
        endBlock = GetBlockAddr(FLASH_BLOCK_OS_START, (g_FlashInfo.dwNumBlocks - 1));
    }
    FlashFormatNANDFlash(startBlock, endBlock);
    StoreBSPCFG(g_pBSPArgs);
    return 0;
}

DWORD MenuReadFlash(MENU_ITEM *p)
{
    if (g_FlashInfo.wDataBytesPerSector == LB_BYTES_PER_PAGE )
    {
        FlashReadNANDFlash(LB_FLASH_BLOCK_OS_START, (g_FlashInfo.dwNumBlocks - 1));
    }
    else
    {
        FlashReadNANDFlash(FLASH_BLOCK_OS_START, (g_FlashInfo.dwNumBlocks - 1));
    }
    return 0;
}
                                  
DWORD MenuReadRelocatoinTable(MENU_ITEM*p)
{
    ReadRelocTable(0);
    return 0;
}

DWORD MenuDumpMFPR(MENU_ITEM * p)
{
    DumpMFPDomainAuto();
    return 0;
}
DWORD MenuCreateRelocationTable(MENU_ITEM *p)
{
    EdbgOutputDebugString ("\r\n");		
    EdbgOutputDebugString ("All blocks (IPL, Kernel, Storage except Reserved Blocks) would be erased. Continue? (Y/N)\r\n");
    if (AreYouSure())
    {
        EdbgOutputDebugString("Please wait ...\r\n");

        if (g_FlashInfo.wDataBytesPerSector == LB_BYTES_PER_PAGE )
        {
            FlashFormatNANDFlash(LB_FLASH_BLOCK_OS_START, (g_FlashInfo.dwNumBlocks - 1));
            CreateRelocTable(0, LB_FLASH_BLOCK_IPL_END+1, FALSE);
            FlashFormatNANDFlash(LB_FLASH_BLOCK_OS_START, (g_FlashInfo.dwNumBlocks - 1));
        }
        else
        {
            FlashFormatNANDFlash(FLASH_BLOCK_OS_START, (g_FlashInfo.dwNumBlocks - 1));
            CreateRelocTable(0, FLASH_BLOCK_IPL_END+1, FALSE);
            FlashFormatNANDFlash(FLASH_BLOCK_OS_START, (g_FlashInfo.dwNumBlocks - 1));
        }
        EdbgOutputDebugString("Please reload KERNEL!\r\n");
    }
    return 0;
}

DWORD MenuCreateEmptyTable(MENU_ITEM *p)
{
    EdbgOutputDebugString ("\r\n");								
    CreateRelocTable(0, 0, TRUE);
    return 0;
}
DWORD MenuReadSector(MENU_ITEM *p)
{
    volatile UINT8 *pDataBuffer = (UINT8 *)OS_RAM_IMAGE_START;
    int sector;
    SectorInfo sectorInfo;
    EdbgOutputDebugString("\r\n");
    sector = GetSectorAddr();
    EdbgOutputDebugString("\r\nRead sector with info %d\r\n", sector);
    if(FlashReadSector(sector, (UINT8 *)pDataBuffer, &sectorInfo ))
    {
        //dumpData((unsigned char *)&sectorInfo, 8, 0);
        dumpSectorInfo(&sectorInfo);
        dumpData((unsigned char *)pDataBuffer, g_FlashInfo.wDataBytesPerSector, 0);
    }
    else
        EdbgOutputDebugString("Failure to Read sector\r\n");

    return 0;
}

DWORD MenuReadPartionTables(MENU_ITEM *p)
{
    volatile UINT8 *pDataBuffer = (UINT8 *)OS_RAM_IMAGE_START;
    SectorInfo sectorInfo;
    SECTOR_ADDR sector;
    EdbgOutputDebugString("\r\n");
    if (g_FlashInfo.wDataBytesPerSector == LB_BYTES_PER_PAGE )
        sector = LB_FLASH_BLOCK_OS_START*g_FlashInfo.wSectorsPerBlock;
    else
        sector = FLASH_BLOCK_OS_START*g_FlashInfo.wSectorsPerBlock;
    if (!FlashReadSector(sector, (UINT8 *)pDataBuffer, &sectorInfo))
    {
        EdbgOutputDebugString("Failure to read sector\r\n");					
    }
    else
    {
        if (((*pDataBuffer) == 0xE9) 
            && (*(pDataBuffer+1) == 0xfd) 
            &&(*(pDataBuffer+2) == 0xff) 
            && (*(pDataBuffer+510) == 0x55) && (*(pDataBuffer+511) == 0xAA)) 
        {
            DumpPartitionTable((UINT8 *)pDataBuffer);
        }
        else
            EdbgOutputDebugString("NO MBR Table available\r\n");
    }
    return 0;					
}

extern BOOL FMD_LockNand(BOOL block, int startAddr, int endAddr);

DWORD MenuLock(MENU_ITEM *p)
{
    EdbgOutputDebugString("!!!Lock flash\r\n");
    if(!FMD_LockNand(TRUE, 0, 0))
        EdbgOutputDebugString("!!!!!!!!!!!failed!\r\n");
    else
        EdbgOutputDebugString("!!!!!!!!!!!success!\r\n");
    return 0;
}

DWORD MenuUnlock(MENU_ITEM *p)
{
    int block;
    EdbgOutputDebugString("!!!Unlock flash: input unlock block num\r\n");
    block = GetSectorAddr();
    if(block <0 || block > 1024)
    {
        EdbgOutputDebugString("!!!!!!!!!!!block num error, should be 0-1024!\r\n");
        return 0;
    }
    if(!FMD_LockNand(TRUE, block, block))
        EdbgOutputDebugString("!!!!!!!!!!!failed!\r\n");
    else
        EdbgOutputDebugString("!!!!!!!!!!!success!\r\n");
    return 0;
}

DWORD MenuErase(MENU_ITEM *p)
{
    volatile UINT8 *pDataBuffer = (UINT8 *)OS_RAM_IMAGE_START;
    int Block;

    EdbgOutputDebugString("\r\n");
//    Block = GetBlockAddr(1, (g_FlashInfo.dwNumBlocks - 1));
    Block = GetBlockAddr(1, (g_FlashInfo.dwNumBlocks - 1));
// For secure boot, need erase NAND FLASH 0,1,2,...    
    EdbgOutputDebugString("Erase Block %d\r\n", Block);
    if(!FlashEraseBlock(Block))
        EdbgOutputDebugString("Erase Block %d FAILURE\r\n", Block);
    return 0;
}

DWORD MenuReadOneBlock(MENU_ITEM *p)
{
    volatile UINT8 *pDataBuffer = (UINT8 *)OS_RAM_IMAGE_START;
    int Block;
    SectorInfo sectorInfo;
    int i;

    EdbgOutputDebugString("\r\n");
    Block = GetBlockAddr(0, (g_FlashInfo.dwNumBlocks - 1));
    EdbgOutputDebugString("Read Block %d\r\n", Block);				
    memset((void *)pDataBuffer, 0x00, g_FlashInfo.dwBytesPerBlock);
    Block = Block*g_FlashInfo.wSectorsPerBlock;
    for (i = 0; i < g_FlashInfo.wSectorsPerBlock; i++)
    {
        if(!FlashReadSector(Block+i, (UINT8 *)pDataBuffer, &sectorInfo))
            EdbgOutputDebugString("ERROR: FMD_ReadSector %d failed\r\n", Block+i);
            dumpData((unsigned char *)pDataBuffer, g_FlashInfo.wDataBytesPerSector, 0);						
    }
    return 0;
}

DWORD MenuWriteOneBlock(MENU_ITEM*p)
{
    volatile UINT8 *pDataBuffer = (UINT8 *)OS_RAM_IMAGE_START;
    int Block;
    SectorInfo sectorInfo;
    int i;

    EdbgOutputDebugString("\r\n");
    Block = GetBlockAddr(1,(g_FlashInfo.dwNumBlocks - 1));
    EdbgOutputDebugString("Write Block %d\r\n", Block);					
    memset((void *)pDataBuffer, 0x01, g_FlashInfo.dwBytesPerBlock);
    memset(&sectorInfo, 0xFF, sizeof(sectorInfo));
    sectorInfo.dwReserved1 = 0;
    sectorInfo.wReserved2 = 0;

    if (FlashEraseBlock(Block) == TRUE)
    {
        Block = Block*g_FlashInfo.wSectorsPerBlock;
        for (i = 0; i < g_FlashInfo.wSectorsPerBlock; i++)
        {
            dumpSectorInfo(&sectorInfo);
            if(!FlashWriteSector(Block+i, (UINT8 *)pDataBuffer, &sectorInfo))
                EdbgOutputDebugString("ERROR: FlashWriteSector %d failed\r\n", Block+i);
            //	dumpData((unsigned char *)pDataBuffer, g_FlashInfo.wDataBytesPerSector, 0);						
        }
    }
    else
        EdbgOutputDebugString("Failure to erase block first\r\n");
    return 0;
}
DWORD MenuFindImgfs(MENU_ITEM *p)
{
    volatile UINT8 *pDataBuffer = (UINT8 *)OS_RAM_IMAGE_START;
    int sector;
    int start;
    int end;
    SectorInfo sectorInfo;
    EdbgOutputDebugString("Enter the start sector:\r\n");
    start = GetSectorAddr();
    EdbgOutputDebugString("\r\n");
    EdbgOutputDebugString("Enter the end sector:\r\n");
    end = GetSectorAddr();
    EdbgOutputDebugString("\r\n");
    for (sector = start; sector <= end; sector++)
    {
        EdbgOutputDebugString(".");
        if(FlashReadSector(sector, (UINT8 *)pDataBuffer, &sectorInfo ))
        {
            if ((*(pDataBuffer+0x2c) == 0x58) && (*(pDataBuffer+0x2D)==0x50) &&
                (*(pDataBuffer+0x2e) == 0x52))
            {
                EdbgOutputDebugString("Read sector with info %d\r\n", sector);
                //dumpData((unsigned char *)&sectorInfo, 8, 0);
                dumpSectorInfo(&sectorInfo);
                dumpData((unsigned char *)pDataBuffer, g_FlashInfo.wDataBytesPerSector, 0);
            }
        }
        else
            EdbgOutputDebugString("Failure to Read sector\r\n");
    }

    return 0;
}

DWORD SetMobm(MENU_ITEM *p)
{
    //SetMOBMSpareArea();
    if (g_FlashInfo.wDataBytesPerSector == LB_BYTES_PER_PAGE )
    {
        // Reserved for MOBM block
        ReservedSpareArea(0, LB_FLASH_BLOCK_EBOOTCFG_START);
    }
    else
    {
        // Reserved for MOBM block
        ReservedSpareArea(0, FLASH_BLOCK_EBOOTCFG_START);
    }
    return 0;
}

DWORD ReserveWTMBlock(MENU_ITEM *p)
{
/* as one example:
From TIM script file:
WTM Save State Entry: 0x06f20000
WTM Save State Backup Entry: 0x06f40000 
0x06f20000 is block 0x06f20000/0x20000 = 0x379 = 889(dec)
0x06f40000 is block 0x06f40000/0x20000 = 0x37A = 890(dec) 
Then we should reserver 889 and 890 */

    int Block;

    EdbgOutputDebugString("\nPlease input 1st Block to reserve \r\n");
    Block = GetBlockAddr(889, (g_FlashInfo.dwNumBlocks - 1));
    EdbgOutputDebugString("\nReserved 1st Block %d\r\n", Block);
    ReservedSpareArea(Block, 1);

    EdbgOutputDebugString("\nPlease input 2nd Block to reserve \r\n");
    Block = GetBlockAddr(890, (g_FlashInfo.dwNumBlocks - 1));
    EdbgOutputDebugString("\nReserved 2nd Block %d\r\n", Block);
    ReservedSpareArea(Block, 1);
	
    
    
    return 0;
}
	
DWORD ReadDword(MENU_ITEM *p)
{
    volatile DWORD *addr;
    DWORD readAddr;

    EdbgOutputDebugString("Enter in a PHYSICAL address: 0x");
    readAddr = GetDWORD();
    // Align
    readAddr &= 0xFFFFFFFC;
    addr = OALPAtoVA(readAddr, FALSE);
    if (addr)
    {
        EdbgOutputDebugString("\r\nPhys 0x%X = Virt 0x%X --> 0x%X\r\n", readAddr, addr, *addr);
    }
    else
    {
        EdbgOutputDebugString("Cannot dereference NULL address! (Failed to translate physical 0x%X to virtual)\r\n", readAddr);
    }
    return 0;
}

MENU_ITEM g_TestMenu[]=
{
        { L'1',L"Sleep",MenuSleep,NULL,NULL,NULL,NULL},
        { L'2',L"Standby Test Loop (infinite)",MenuStandby,NULL,NULL,NULL,NULL},
        //{ L'3',L"Mini-LCD Test Loop (infinite)",MenuMiniLCD,NULL,NULL,NULL,NULL},
        { L'3',L"Mini-LCD Test Loop (infinite)",MenuLCD,NULL,NULL,NULL,NULL},
        { L'4',L"Sleep and D0CS Test",MenuSleepD0CSTest,(VOID*)FALSE,NULL,NULL,NULL},		
        { L'R',L"Load OS image from NAND Flash",MenuLoadOS,(VOID*)FALSE,NULL,NULL,NULL},
        { L'M',L"Load OS image with Jump",MenuLoadOS,(VOID*)TRUE,NULL,NULL,NULL},
        { L'J',L"Jump to OS image",CallP1VOID,JumpToImageDDR,NULL,NULL,NULL},
        { L'A',L"Read DDR",CallP1VOID,ReadSimpleDDR,NULL,NULL,NULL},
        { L'B',L"Write DDR",CallP1VOID,WriteSimpleDDR,NULL,NULL,NULL},
        { L'C',L"Write/Read DDR",WriteRdDDR,(VOID*)NUM_READ_WRITES,NULL,NULL,NULL},
        { L'E',L"Write/Read DDR quick test",WriteRdDDR,(VOID*)NUM_READ_WRITES_SHORT,NULL,NULL,NULL},
        { L'D',L"Dump Registers DDR",CallP1VOID,DumpRegistersDDR,NULL,NULL,NULL},
        { L'F',L"Dump EBootLoader image",CallP1VOID,DumpBootLoaderImage,NULL,NULL,NULL},
        { L'G',L"Format Flash ",MenuFormat,NULL,NULL,NULL,NULL},
        { L'H',L"Read Flash",MenuReadFlash,NULL,NULL,NULL,NULL},		
        { L'5',L"Dump MFPRs",MenuDumpMFPR,NULL,NULL,NULL,NULL},
        { L'L',L"LED Test",CallP1VOID,TestLED,NULL,NULL,NULL},		
        { L'N',L"Read Relocation Table",MenuReadRelocatoinTable,NULL,NULL,NULL,NULL},				
        { L'P',L"Read Sector",MenuReadSector,NULL,NULL,NULL,NULL},
        { L'8',L"Lock Flash",MenuLock,NULL,NULL,NULL,NULL},
        { L'9',L"Unlock Sector",MenuUnlock,NULL,NULL,NULL,NULL},		
        { L'Q',L"Read Partition Tables",MenuReadPartionTables,NULL,NULL,NULL,NULL},		
        { L'O',L"Create Relocation Table",MenuCreateRelocationTable,NULL,NULL,NULL,NULL},		
        { L'I',L"Create Empty Relocation Table",MenuCreateEmptyTable,NULL,NULL,NULL,NULL},		
        { L'K',L"Find IMGFS compression value",MenuFindImgfs,NULL,NULL,NULL,NULL},		
        { L'S',L"Erase one block",MenuErase,NULL,NULL,NULL,NULL},		
        { L'T',L"Read one block",MenuReadOneBlock,NULL,NULL,NULL,NULL},		
        { L'U',L"Write one block",MenuWriteOneBlock,NULL,NULL,NULL,NULL},		
        { L'V',L"Set MOBM block",SetMobm,NULL,NULL},		
        { L'W',L"Reserve WTM Blocks for WTPSP",ReserveWTMBlock,NULL,NULL},		        
        { L'0',L"Read 32-bit DWORD from memory",ReadDword,NULL,NULL,NULL,NULL},
        { L'X',L"Exit",NULL,NULL,NULL,NULL,NULL},
        { 0,NULL,NULL,NULL,NULL,NULL,NULL}
};

